/**************************************************************************************************
* Title: SmurfsRigidBody.cpp
* Author: Gael Huber
* Description: Holds a rigid body used for physics and collision systems.
**************************************************************************************************/
#include "SmurfsRigidBody.h"

/**************************************************************************************************
* Constructor
**************************************************************************************************/
RigidBody::RigidBody(void) {
}

/**************************************************************************************************
* Constructor defining dimensions
**************************************************************************************************/
RigidBody::RigidBody(Vector3 halfWidths, float mass) {
	this->axes = Quaternion();
	this->halfWidths = halfWidths;
	this->mass = mass;

	// Define the moment of inertia tensor
	float w = halfWidths.x * 2.0f;
	float w2 = halfWidths.x * halfWidths.x;
	float h = halfWidths.z * 2.0f;
	float h2 = halfWidths.z * halfWidths.z;
	float d = halfWidths.y * 2.0f;
	float d2 = halfWidths.y * halfWidths.y;

	inertiaTensor = Matrix(3, 3);
	inertiaTensor.matrix[0][0] = mass * w * w / 12.0f + mass * w2;
	inertiaTensor.matrix[1][1] = mass * w * w / 12.0f + mass * w2;
	inertiaTensor.matrix[2][2] = mass * w * w / 12.0f * mass * w2;

	// Calculat the inertia tensor in world coordinates (just for kicks at this point)
	worldInertiaTensor = Matrix(3, 3);
	Matrix m = Matrix(3, 3);
	m.matrix[0][0] = 1 - 2*axes.y*axes.y - 2*axes.z*axes.z;
	m.matrix[0][1] = 2*axes.x*axes.y + 2*axes.w*axes.z;
	m.matrix[0][2] = 2*axes.x*axes.z - 2*axes.w*axes.y;

	m.matrix[1][0] = 2*axes.x*axes.y - 2*axes.w*axes.z;
	m.matrix[1][1] = 1 - 2*axes.x*axes.x - 2*axes.z*axes.z;
	m.matrix[1][2] = 2*axes.y*axes.z + 2*axes.w*axes.x;
	
	m.matrix[2][0] = 2*axes.x*axes.z + 2*axes.w*axes.y;
	m.matrix[2][1] = 2*axes.y*axes.z - 2*axes.w*axes.x;
	m.matrix[2][2] = 1 - 2*axes.x*axes.x - 2*axes.y*axes.y;
	
	worldInertiaTensor = (m * inertiaTensor) * m.getTranspose();

	// Setup control points
	controls[0] = Vector3();	controls[0].x -= halfWidths.x;
	controls[1] = Vector3();
	controls[2] = Vector3();	controls[2].x += halfWidths.x;
}

/**************************************************************************************************
* Destructor
**************************************************************************************************/
RigidBody::~RigidBody(void) {
}

Matrix RigidBody::getWorldInertiaTensor(void) {
	// Calculat the inertia tensor in world coordinates
	Matrix m = Matrix(3, 3);
	m.matrix[0][0] = 1 - 2*axes.y*axes.y - 2*axes.z*axes.z;
	m.matrix[0][1] = 2*axes.x*axes.y + 2*axes.w*axes.z;
	m.matrix[0][2] = 2*axes.x*axes.z - 2*axes.w*axes.y;

	m.matrix[1][0] = 2*axes.x*axes.y - 2*axes.w*axes.z;
	m.matrix[1][1] = 1 - 2*axes.x*axes.x - 2*axes.z*axes.z;
	m.matrix[1][2] = 2*axes.y*axes.z + 2*axes.w*axes.x;
	
	m.matrix[2][0] = 2*axes.x*axes.z + 2*axes.w*axes.y;
	m.matrix[2][1] = 2*axes.y*axes.z - 2*axes.w*axes.x;
	m.matrix[2][2] = 1 - 2*axes.x*axes.x - 2*axes.y*axes.y;
	
	worldInertiaTensor = (m * inertiaTensor) * m.getTranspose();
	return worldInertiaTensor;
}